//
// Created by weiyixing on 25-9-11.
// Enhanced version with better error handling and Python bindings support
//

#include "scvCameraCore.h"
#include "libobsensor/hpp/Error.hpp"
#include "libobsensor/hpp/Frame.hpp"
#include "libobsensor/hpp/Context.hpp"
#include "log.h"
#include <iostream>
#include <algorithm>

scvCameraCore::scvCameraCore() : deviceSerial_("") {
    // 使用默认配置
}

scvCameraCore::scvCameraCore(const std::string& deviceSerial) : deviceSerial_(deviceSerial) {
    // 使用指定设备序列号
}

scvCameraCore::~scvCameraCore() {
    shutdown();
}

SCVErrorCode scvCameraCore::initialize() {
    return initialize(colorProfile_, depthProfile_);
}

SCVErrorCode scvCameraCore::initialize(const ColorProfile& colorProfile, const DepthProfile& depthProfile) {
    if (initialized_) {
        return SUCCESS;
    }

    // 验证配置参数
    if (!validateProfile(colorProfile) || !validateProfile(depthProfile)) {
        handleError(ERROR_INVALID_PARAMETER, "Invalid profile parameters");
        return ERROR_INVALID_PARAMETER;
    }

    colorProfile_ = colorProfile;
    depthProfile_ = depthProfile;

    try {
        // 创建Context
        context_ = std::make_shared<ob::Context>();
        
        // 获取设备列表
        auto deviceList = context_->queryDeviceList();
        if (deviceList->deviceCount() == 0) {
            handleError(ERROR_DEVICE_NOT_FOUND, "No Orbbec devices found");
            return ERROR_DEVICE_NOT_FOUND;
        }

        // 选择设备
        if (deviceSerial_.empty()) {
            device_ = deviceList->getDevice(0);
        } else {
            for (uint32_t i = 0; i < deviceList->deviceCount(); i++) {
                auto dev = deviceList->getDevice(i);
                if (dev->getDeviceInfo()->serialNumber() == deviceSerial_) {
                    device_ = dev;
                    break;
                }
            }
            if (!device_) {
                handleError(ERROR_DEVICE_NOT_FOUND, "Device with serial " + deviceSerial_ + " not found");
                return ERROR_DEVICE_NOT_FOUND;
            }
        }

        // 创建Pipeline
        pipe_ = std::make_shared<ob::Pipeline>(device_);
        config_ = std::make_shared<ob::Config>();

        // 初始化配置
        SCVErrorCode result = initProfile();
        if (result != SUCCESS) {
            return result;
        }

        initialized_ = true;
        return SUCCESS;

    } catch (const ob::Error& e) {
        handleError(ERROR_INIT_FAILED, "Initialization failed: " + std::string(e.getMessage()));
        return ERROR_INIT_FAILED;
    }
}

void scvCameraCore::shutdown() {
    if (running_) {
        stopStreams();
    }
    
    if (pipe_) {
        try {
            pipe_->stop();
        } catch (...) {
            // 忽略停止时的错误
        }
    }
    
    initialized_ = false;
    running_ = false;
}

std::vector<DeviceInfo> scvCameraCore::enumerateDevices() {
    std::vector<DeviceInfo> devices;
    
    try {
        auto context = std::make_shared<ob::Context>();
        auto deviceList = context->queryDeviceList();
        
        for (uint32_t i = 0; i < deviceList->deviceCount(); i++) {
            auto device = deviceList->getDevice(i);
            auto deviceInfo = device->getDeviceInfo();
            
            DeviceInfo info;
            info.name = deviceInfo->name();
            info.serialNumber = deviceInfo->serialNumber();
            info.firmwareVersion = deviceInfo->firmwareVersion();
            info.isConnected = true; // 如果能获取到设备信息，说明已连接
            
            devices.push_back(info);
        }
    } catch (const ob::Error& e) {
        std::cerr << "Error enumerating devices: " << e.getMessage() << std::endl;
    }
    
    return devices;
}

SCVErrorCode scvCameraCore::connectDevice(const std::string& deviceSerial) {
    deviceSerial_ = deviceSerial;
    if (initialized_) {
        shutdown();
    }
    return initialize();
}

void scvCameraCore::disconnectDevice() {
    shutdown();
}

DeviceInfo scvCameraCore::getCurrentDeviceInfo() const {
    DeviceInfo info;
    if (device_) {
        auto deviceInfo = device_->getDeviceInfo();
        info.name = deviceInfo->name();
        info.serialNumber = deviceInfo->serialNumber();
        info.firmwareVersion = deviceInfo->firmwareVersion();
        info.isConnected = true;
    }
    return info;
}

SCVErrorCode scvCameraCore::startStreams() {
    if (!initialized_) {
        handleError(ERROR_INIT_FAILED, "Camera not initialized");
        return ERROR_INIT_FAILED;
    }

    if (running_) {
        return SUCCESS;
    }

    try {
        pipe_->start(config_);
        running_ = true;
        return SUCCESS;
    } catch (const ob::Error& e) {
        handleError(ERROR_STREAM_NOT_AVAILABLE, "Failed to start streams: " + std::string(e.getMessage()));
        return ERROR_STREAM_NOT_AVAILABLE;
    }
}

void scvCameraCore::stopStreams() {
    if (running_ && pipe_) {
        try {
            pipe_->stop();
        } catch (...) {
            // 忽略停止时的错误
        }
        running_ = false;
    }
}

SCVErrorCode scvCameraCore::restartStreams() {
    stopStreams();
    return startStreams();
}

SCVErrorCode scvCameraCore::getAlignColorDepthMat(cv::Mat& colorMat, cv::Mat& depthMat) {
    if (!running_) {
        handleError(ERROR_INIT_FAILED, "Streams not started");
        return ERROR_INIT_FAILED;
    }

    try {
        int fps = std::min(cFPS_, dFPS_);
        int timeout_ms = 1000 / fps;
        auto frameSet = pipe_->waitForFrames(timeout_ms);
        
        if (frameSet == nullptr) {
            if(context_->queryDeviceList()->deviceCount()==0){
                handleError(ERROR_DEVICE_DISCONNECTED, "Device disconnected");
                return ERROR_DEVICE_DISCONNECTED;
            }
            handleError(ERROR_TIMEOUT, "Frame timeout");
            return ERROR_TIMEOUT;
        }

        auto colorFrame = frameSet->colorFrame();
        auto depthFrame = frameSet->depthFrame();
        
        if (colorFrame == nullptr || depthFrame == nullptr) {
            handleError(ERROR_STREAM_NOT_AVAILABLE, "Missing color or depth frame");
            return ERROR_STREAM_NOT_AVAILABLE;
        }

        // 对齐处理
        auto newFrame = align_->process(frameSet);
        auto newFrameSet = newFrame->as<ob::FrameSet>();
        colorFrame = newFrameSet->colorFrame();
        depthFrame = newFrameSet->depthFrame();

        processFrame(colorFrame, colorMat);
        processFrame(depthFrame, depthMat);

        // 调用回调函数
        if (frameCallback_) {
            frameCallback_(colorMat, depthMat);
        }

        return SUCCESS;

    } catch (const ob::Error& e) {
        handleError(ERROR_DEVICE_DISCONNECTED, "Device error: " + std::string(e.getMessage()));
        return ERROR_DEVICE_DISCONNECTED;
    }
}

SCVErrorCode scvCameraCore::getColorMat(cv::Mat& colorMat) {
    if (!running_) {
        handleError(ERROR_INIT_FAILED, "Streams not started");
        return ERROR_INIT_FAILED;
    }

    try {
        int timeout_ms = 1000 / cFPS_;
        auto frameSet = pipe_->waitForFrames(timeout_ms);
        
        if (frameSet == nullptr) {
            handleError(ERROR_TIMEOUT, "Frame timeout");
            return ERROR_TIMEOUT;
        }

        auto colorFrame = frameSet->colorFrame();
        if (colorFrame == nullptr) {
            handleError(ERROR_STREAM_NOT_AVAILABLE, "Missing color frame");
            return ERROR_STREAM_NOT_AVAILABLE;
        }

        processFrame(colorFrame, colorMat);
        return SUCCESS;

    } catch (const ob::Error& e) {
        handleError(ERROR_DEVICE_DISCONNECTED, "Device error: " + std::string(e.getMessage()));
        return ERROR_DEVICE_DISCONNECTED;
    }
}

SCVErrorCode scvCameraCore::getDepthMat(cv::Mat& depthMat) {
    if (!running_) {
        handleError(ERROR_INIT_FAILED, "Streams not started");
        return ERROR_INIT_FAILED;
    }

    try {
        int timeout_ms = 1000 / dFPS_;
        auto frameSet = pipe_->waitForFrames(timeout_ms);
        
        if (frameSet == nullptr) {
            handleError(ERROR_TIMEOUT, "Frame timeout");
            return ERROR_TIMEOUT;
        }

        auto depthFrame = frameSet->depthFrame();
        if (depthFrame == nullptr) {
            handleError(ERROR_STREAM_NOT_AVAILABLE, "Missing depth frame");
            return ERROR_STREAM_NOT_AVAILABLE;
        }

        processFrame(depthFrame, depthMat);
        return SUCCESS;

    } catch (const ob::Error& e) {
        handleError(ERROR_DEVICE_DISCONNECTED, "Device error: " + std::string(e.getMessage()));
        return ERROR_DEVICE_DISCONNECTED;
    }
}

SCVErrorCode scvCameraCore::setColorProfile(const ColorProfile& profile) {
    if (!validateProfile(profile)) {
        handleError(ERROR_INVALID_PARAMETER, "Invalid color profile");
        return ERROR_INVALID_PARAMETER;
    }

    colorProfile_ = profile;
    
    if (initialized_) {
        return restartStreams();
    }
    
    return SUCCESS;
}

SCVErrorCode scvCameraCore::setDepthProfile(const DepthProfile& profile) {
    if (!validateProfile(profile)) {
        handleError(ERROR_INVALID_PARAMETER, "Invalid depth profile");
        return ERROR_INVALID_PARAMETER;
    }

    depthProfile_ = profile;
    
    if (initialized_) {
        return restartStreams();
    }
    
    return SUCCESS;
}

void scvCameraCore::setFrameCallback(const FrameCallback& callback) {
    frameCallback_ = callback;
}

void scvCameraCore::setErrorCallback(const ErrorCallback& callback) {
    errorCallback_ = callback;
}

void scvCameraCore::removeFrameCallback() {
    frameCallback_ = nullptr;
}

void scvCameraCore::removeErrorCallback() {
    errorCallback_ = nullptr;
}

bool scvCameraCore::isColorStreamEnabled() const {
    return config_ != nullptr;
}

bool scvCameraCore::isDepthStreamEnabled() const {
    return config_ != nullptr;
}

std::string scvCameraCore::getErrorString(SCVErrorCode error) {
    switch (error) {
        case SUCCESS: return "Success";
        case ERROR_INIT_FAILED: return "Initialization failed";
        case ERROR_DEVICE_NOT_FOUND: return "Device not found";
        case ERROR_STREAM_NOT_AVAILABLE: return "Stream not available";
        case ERROR_INVALID_PARAMETER: return "Invalid parameter";
        case ERROR_TIMEOUT: return "Timeout";
        case ERROR_DEVICE_DISCONNECTED: return "Device disconnected";
        default: return "Unknown error";
    }
}

void scvCameraCore::setLogLevel(int level) {
    // 这里可以设置日志级别
    // 具体实现取决于使用的日志库
}

SCVErrorCode scvCameraCore::initProfile() {
    return initProfile(colorProfile_, depthProfile_);
}

SCVErrorCode scvCameraCore::initProfile(const ColorProfile& cProfile, const DepthProfile& dProfile) {
    try {
        cFPS_ = cProfile.fps;
        dFPS_ = dProfile.fps;

        // 配置彩色流
        config_->enableVideoStream(OB_STREAM_COLOR);

        // 配置深度流
        config_->enableVideoStream(OB_STREAM_DEPTH);

        // 创建对齐器
        OBStreamType align_to_stream = OB_STREAM_COLOR;
        align_ = std::make_shared<ob::Align>(align_to_stream);

        return SUCCESS;

    } catch (const ob::Error& e) {
        handleError(ERROR_INIT_FAILED, "Profile initialization failed: " + std::string(e.getMessage()));
        return ERROR_INIT_FAILED;
    }
}

void scvCameraCore::processFrame(const std::shared_ptr<ob::Frame>& frame, cv::Mat& rstMat) {
    if (frame == nullptr) {
        return;
    }

    if (frame->type() == OB_FRAME_COLOR) {
        auto videoFrame = frame->as<ob::VideoFrame>();
        switch (videoFrame->format()) {
            case OB_FORMAT_MJPG: {
                cv::Mat rawMat(1, videoFrame->dataSize(), CV_8UC1, videoFrame->data());
                rstMat = cv::imdecode(rawMat, 1);
            } break;
            case OB_FORMAT_NV21: {
                cv::Mat rawMat(videoFrame->height() * 3 / 2, videoFrame->width(), CV_8UC1, videoFrame->data());
                cv::cvtColor(rawMat, rstMat, cv::COLOR_YUV2BGR_NV21);
            } break;
            case OB_FORMAT_YUYV:
            case OB_FORMAT_YUY2: {
                cv::Mat rawMat(videoFrame->height(), videoFrame->width(), CV_8UC2, videoFrame->data());
                cv::cvtColor(rawMat, rstMat, cv::COLOR_YUV2BGR_YUY2);
            } break;
            case OB_FORMAT_RGB: {
                cv::Mat rawMat(videoFrame->height(), videoFrame->width(), CV_8UC3, videoFrame->data());
                cv::cvtColor(rawMat, rstMat, cv::COLOR_RGB2BGR);
            } break;
            case OB_FORMAT_RGBA: {
                cv::Mat rawMat(videoFrame->height(), videoFrame->width(), CV_8UC4, videoFrame->data());
                cv::cvtColor(rawMat, rstMat, cv::COLOR_RGBA2BGRA);
            } break;
            case OB_FORMAT_BGRA: {
                rstMat = cv::Mat(videoFrame->height(), videoFrame->width(), CV_8UC4, videoFrame->data());
            } break;
            case OB_FORMAT_UYVY: {
                cv::Mat rawMat(videoFrame->height(), videoFrame->width(), CV_8UC2, videoFrame->data());
                cv::cvtColor(rawMat, rstMat, cv::COLOR_YUV2BGR_UYVY);
            } break;
            case OB_FORMAT_I420: {
                cv::Mat rawMat(videoFrame->height() * 3 / 2, videoFrame->width(), CV_8UC1, videoFrame->data());
                cv::cvtColor(rawMat, rstMat, cv::COLOR_YUV2BGR_I420);
            } break;
            default:
                break;
        }
    }
    else if (frame->type() == OB_FRAME_DEPTH) {
        auto videoFrame = frame->as<ob::VideoFrame>();
        if (videoFrame->format() == OB_FORMAT_Y16 || videoFrame->format() == OB_FORMAT_Z16) {
            cv::Mat rawMat = cv::Mat(videoFrame->height(), videoFrame->width(), CV_16UC1, videoFrame->data());
            // 深度帧像素值乘以缩放因子得到毫米单位的距离
            float scale = videoFrame->as<ob::DepthFrame>()->getValueScale();

            // 阈值限制到5.12米
            cv::threshold(rawMat, rstMat, 5120.0f / scale, 0, cv::THRESH_TRUNC);
        }
    }
}

void scvCameraCore::handleError(SCVErrorCode error, const std::string& message) {
    if (errorCallback_) {
        errorCallback_(error, message);
    } else {
        std::cerr << "SCV Error [" << error << "]: " << message << std::endl;
    }
}

bool scvCameraCore::validateProfile(const ColorProfile& profile) {
    return profile.width > 0 && profile.height > 0 && 
           profile.fps > 0 && profile.fps <= 60 &&
           profile.format >= MJPG && profile.format <= I420;
}

bool scvCameraCore::validateProfile(const DepthProfile& profile) {
    return profile.width > 0 && profile.height > 0 && 
           profile.fps > 0 && profile.fps <= 60;
}

OBFormat scvCameraCore::getOBFormat(SCVColorFormat format) {
    switch (format) {
        case MJPG: return OB_FORMAT_MJPEG;
        case RGB888: return OB_FORMAT_RGB;
        case BGRA: return OB_FORMAT_RGBA;
        case YUYV: return OB_FORMAT_YUYV;
        case NV21: return OB_FORMAT_NV21;
        case I420: return OB_FORMAT_I420;
        default: return OB_FORMAT_RGB;
    }
}